package com.jmex.physics.impl.jbullet;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.CompoundShape;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.linearmath.Transform;
import com.jme.math.Quaternion;
import com.jme.math.Vector3f;
import com.jme.scene.Node;
import com.jme.scene.Spatial;
import com.jmex.physics.DynamicPhysicsNode;
import com.jmex.physics.Joint;
import com.jmex.physics.PhysicsCollisionGeometry;
import com.jmex.physics.PhysicsSpace;
import com.jmex.physics.impl.jbullet.geometry.JBulletGeometry;
import com.jmex.physics.impl.jbullet.util.VecmathConverter;

public class JBulletDynamicPhysicsNode extends DynamicPhysicsNode implements
JBulletPhysicsNode {

	private JBulletPhysicsSpace space;
	private RigidBody body;
	private NodeLockedMotionState motionState;
	private List<JBulletGeometry> geoms;
	private float mass=1f;
	private Vector3f myPrivateGravity = null;
	private boolean affectedByGravity=true;
	private boolean dirty = true;
	
	private Set<Joint> myJoints = new HashSet<Joint>();
	
	private Vector3f angularVelocity = new Vector3f(0f,0f,0f);
	private Vector3f linearVelocity = new Vector3f(0f,0f,0f);
	private Vector3f centerOfMass = new Vector3f(0f,0f,0f);
	
	private Quaternion previousRotation = new Quaternion();
	private Vector3f previousTranslation = new Vector3f();
	
	@Override
	public void setLocalRotation(Quaternion quaternion) {
		setLocalRotation(quaternion,false);
	}

	@Override
	public void setLocalTranslation(Vector3f localTranslation) {
		setLocalTranslation(localTranslation,false);
	}

	public void setLocalRotation(Quaternion quaternion, boolean internalUpdate) {
		super.setLocalRotation(quaternion);
		if(internalUpdate)
			previousRotation.set(quaternion);
	}

	public void setLocalTranslation(Vector3f localTranslation, boolean internalUpdate) {
		super.setLocalTranslation(localTranslation);
		if(internalUpdate)
			previousTranslation.set(localTranslation);
	}
	
	public void correctInterpolationsForNonPhysicsMovement()
	{
		if(!(getLocalTranslation().equals(previousTranslation) &&
			 getLocalRotation().equals(previousRotation)))
		{
			 body.getInterpolationWorldTransform().set(body.getWorldTransform());
			 body.setWorldTransform(body.getWorldTransform()); 
			 //Looks dumb, but fixes a stupid incorrect encapsulation issue 
			 //within JBullet dealing with the 'worldTransform' field.
		}
	}
	
	public JBulletDynamicPhysicsNode(String name, JBulletPhysicsSpace space)
	{
		super(name);
		this.space = space;
		geoms = new ArrayList<JBulletGeometry>();
		//motionState = new NodeLockedMotionState(this);
	}

	public JBulletDynamicPhysicsNode(JBulletPhysicsSpace space)
	{
		this(null,space);
	}
	
	public RigidBody getBody() {
		return body;
	}

	@Override
	public void addForce(Vector3f force, Vector3f at) {
		if(body!=null)
			body.applyForce(VecmathConverter.convert(force),
						VecmathConverter.convert(at));
	}

	@Override
	public void addTorque(Vector3f torque) {
		if(body!=null)
			body.applyTorque(VecmathConverter.convert(torque));
	}

	@Override
	public void clearForce() {
		if(body!=null)
			body.clearForces();
		//TODO Figure out if there's a way to clear torque and linear forces separately.
	}

	@Override
	public void clearTorque() {
		if(body!=null)
			body.clearForces();
		//TODO Figure out if there's a way to clear torque and linear forces separately.
	}

	@Override
	public void computeMass() {
        updateGeometricState( 0, true ); // update geom positions
		float density = getMaterial().getDensity();
		if(geoms==null || geoms.size()==0)
			throw new IllegalArgumentException("Cannot compute mass when no collision geometries have been added.");
		mass=0;
		for(JBulletGeometry geom : geoms)
			mass+=geom.getVolume() * density;
 		setDirty(true);
	}

	@Override
	public Vector3f getAngularVelocity(Vector3f store) {
		if(body==null)
			return angularVelocity;
		return VecmathConverter.convert(body.getAngularVelocity());
	}

	@Override
	public Vector3f getCenterOfMass(Vector3f store) {
		if(body==null)
			return centerOfMass;
		return VecmathConverter.convert(body.getCenterOfMassPosition());
	}

	@Override
	public Vector3f getForce(Vector3f store) {
		//TODO make this a reflective call and return JBullet code to normal.
		if(body==null)
			return new Vector3f(0f,0f,0f);
		//return VecmathConverter.convert(body.getTotalForce());
		return new Vector3f(0f,0f,0f);
	}

	@Override
	public Vector3f getLinearVelocity(Vector3f store) {
		if(body==null)
			return linearVelocity;
		return VecmathConverter.convert(body.getLinearVelocity());
	}

	@Override
	public float getMass() {
		return mass;
	}

	@Override
	public Vector3f getTorque(Vector3f store) {
		//TODO make this a reflective call and return JBullet code to normal.
		if(body==null)
			return new Vector3f(0f,0f,0f);
		//return VecmathConverter.convert(body.getTotalTorque());
		return new Vector3f(0f,0f,0f);
	}

	@Override
	public boolean isAffectedByGravity() {
		return affectedByGravity;
	}

	@Override
	public void rest() {
		if(body!=null)
			body.setActivationState(CollisionObject.ACTIVE_TAG);
	}

	@Override
	public void setAffectedByGravity(boolean value) {
		if(!value)
		{
			affectedByGravity = false;
			if(body!=null)
				body.setGravity(BulletGlobals.ZERO_VECTOR3);
		}
		else
		{
			affectedByGravity = true;
			if(body==null)
				return;
			if(myPrivateGravity==null)
			{
				((JBulletPhysicsSpace)getSpace()).dynamicsWorld.removeRigidBody(body);
				((JBulletPhysicsSpace)getSpace()).dynamicsWorld.addRigidBody(body);
			} else {
				body.setGravity(VecmathConverter.convert(myPrivateGravity));
			}
		}
	}

	@Override
	public void setAngularVelocity(Vector3f velocity) {
		angularVelocity.set(velocity);
		if(body!=null)
			body.setAngularVelocity(VecmathConverter.convert(velocity));
	}

	@Override
	public void setCenterOfMass(Vector3f value) {
		if(centerOfMass==null)
			centerOfMass=new Vector3f();
		centerOfMass.set(value);
		Transform t = new Transform();
		t.transform(VecmathConverter.convert(value));
//		if(body!=null)
//			body.setCenterOfMassTransform(t);
	}

	@Override
	public void setLinearVelocity(Vector3f velocity) {
		linearVelocity.set(velocity);
		if(body!=null)
			body.setLinearVelocity(VecmathConverter.convert(velocity));
	}

	@Override
	public void setMass(float value) {
		mass=value;
		setDirty(true);
	}

	@Override
	public void unrest() {
		if(body!=null)
			body.activate();
	}

	@Override
	public PhysicsSpace getSpace() {
		return space;
	}

	@Override
	public boolean isResting() {
		if(body==null)
			return false;
		return body.getActivationState() == CollisionObject.ISLAND_SLEEPING;
	}

	public JBulletCollisionGroup getCollisionGroup()
	{
		return (JBulletCollisionGroup) super.getCollisionGroup();
	}
	
    public void setCollisionGroup( JBulletCollisionGroup value ) {
        boolean wasActive = isActive();
        setActive( false );
        super.setCollisionGroup( value );
        setActive( wasActive );
    }

	@Override
	public boolean setActive(boolean value) {
		boolean wasActive = isActive();
		boolean changed = super.setActive(value);
		if(value && !wasActive && body!=null)
			((JBulletPhysicsSpace)getSpace()).dynamicsWorld.addRigidBody(body);
		else if(!value && wasActive && body!=null)
			((JBulletPhysicsSpace)getSpace()).dynamicsWorld.removeRigidBody(body);
		return changed;	
	}

	@Override
	public int attachChild(Spatial child) {
        Node oldParent = child != null ? child.getParent() : null;
        int index = super.attachChild( child );
        if ( child instanceof JBulletGeometry ) {
        	JBulletGeometry jBulletGeom = (JBulletGeometry) child;
            if ( oldParent != this ) {
            	geoms.add(jBulletGeom);
            	setDirty(true);
            }
        }
        else if ( child instanceof PhysicsCollisionGeometry ) {
            throw new IllegalArgumentException( "Cannot handle geometries from different implementations!" );
        }
        return index;
	}
	
	public void rebuildRigidBody()
	{
		if(body!=null)
		{
			angularVelocity = VecmathConverter.convert(body.getAngularVelocity());
			linearVelocity = VecmathConverter.convert(body.getLinearVelocity());
			((JBulletPhysicsSpace)getSpace()).dynamicsWorld.removeRigidBody(body);
			body.destroy();
		}

		if(geoms==null)
			return;
		
		CollisionShape shape;
		shape = new CompoundShape();
		float massTemp;
		javax.vecmath.Vector3f inertia = new javax.vecmath.Vector3f(0f,0f,0f);
		for(JBulletGeometry geom : geoms)
		{
			geom.getJBulletShape().setLocalScaling(VecmathConverter.convert(geom.getLocalScale().mult(getLocalScale())));
			massTemp = geom.getVolume() * getMaterial().getDensity();
			geom.getJBulletShape().calculateLocalInertia(massTemp, inertia);
			Transform t = new Transform();
			t.setIdentity();
			t.setRotation(VecmathConverter.convert(geom.getLocalRotation()));
			t.origin.set(VecmathConverter.convert(geom.getLocalTranslation()));
			((CompoundShape)shape).addChildShape(t,geom.getJBulletShape());
		}
		
		motionState = new NodeLockedMotionState(this);
		
		RigidBodyConstructionInfo info = 
			new RigidBodyConstructionInfo(mass,
										  motionState,
										  shape,
										  inertia);
		//TODO Figure out a way to handle friction correctly.
		info.friction=getMaterial().getContactHandlingDetails(null).getMu();
		//TODO Figure out how to handle mapping all the remaining construction info properties to material type.
		
		body = new NodeLockedRigidBody(this,info);
		
		body.setAngularVelocity(VecmathConverter.convert(angularVelocity));
		body.setLinearVelocity(VecmathConverter.convert(linearVelocity));
//		if(centerOfMass!=null)
//			setCenterOfMass(centerOfMass);
		
		if(isActive())
			((JBulletPhysicsSpace)getSpace()).dynamicsWorld.addRigidBody(body);
		if(!affectedByGravity)
			body.setGravity(BulletGlobals.ZERO_VECTOR3);
		else if(myPrivateGravity!=null)
			body.setGravity(VecmathConverter.convert(myPrivateGravity));
		
		setDirty(false);
	}
	
	@Override
	public int detachChild(Spatial child) {
        Node oldParent = child != null ? child.getParent() : null;
        int index = super.detachChild( child );
        if ( child instanceof JBulletGeometry ) {
        	JBulletGeometry jBulletGeom = (JBulletGeometry) child;
            if ( oldParent == this ) {
                geoms.remove(jBulletGeom);
                setDirty(true);
            }
        }
        return index;
	}

	
	
	@Override
	public void generatePhysicsGeometry() {
		super.generatePhysicsGeometry();
    	System.out.println("Rebuilding rigid bodiy after dynamic node 'generatePhysicsGeometry'.");
		rebuildRigidBody();
	}

	@Override
	public void generatePhysicsGeometry(boolean useTriangleAccurateGeometries) {
		super.generatePhysicsGeometry(useTriangleAccurateGeometries);
    	System.out.println("Rebuilding rigid bodiy after dynamic node 'generatePhysicsGeometry'.");
		rebuildRigidBody();
	}

	public boolean isDirty() {
		if(dirty)
			return true;
		for(JBulletGeometry geom : geoms)
			if(geom.isDirty())
				return true;
		return false;
	}

	public void setDirty(boolean dirty) {
		this.dirty = dirty;
		if(!dirty)
			for(JBulletGeometry geom : geoms)
				geom.setDirty(false);
	}

	public void updateWorldVectors() {
		if(getParent()!=null)
			getParent().updateWorldVectors();
		super.updateWorldVectors();
	}
	
	public void addParticipatingJoint(Joint joint)
	{
		myJoints.add(joint);
	}
	
	public void removeParticipatingJoint(Joint joint)
	{
		myJoints.remove(joint);
	}

	public Set<Joint> getMyJoints() {
		return Collections.unmodifiableSet(myJoints);
	}
}
